#Author : zhangruonan

from __future__ import division
import cv2
import numpy as np

def pixel_to_world(point):
# # #世界坐标系下四个点
    zconst=0
    object_3d_points = np.array(([16.889012347222167, -8.180261270690034 ,0] ,
 [26.813783012521057 ,-13.163580421733593 ,0] ,
 [27.32877244292133 ,-19.20021016988661,0], 
 [ 23.87174082974574, -29.41011136993275,0], 
 [16.424201374952883 ,-22.5696115383423 ,0], 
 [22.66217591070119, -17.918955426261363 ,0], 
 [ 16.564172861337077 ,-9.9758497315821 ,0], 
 [11.86060272838353 ,-11.651315721095646,0]
), dtype=np.double)

    #对应像素坐标系下四个像素点,cam01
    object_2d_point = np.array(([73, 358], [199,151], [418,85], [867,58],[1063,167],[720,183],[496,347],[819,437]),dtype=np.double)
    camera_matrix = np.array((
                           [[ 1990.58, 0,645.936], 
                           [0,1990.5822, 352.825],
                           [0,       0,        1]]), dtype=np.double)
    
    dist_coefs = np.array([ -8.5540304078814322e-01, 2.3438623880456033e+00,
       7.5879378654809234e-03, 1.3178544078814979e-03,
       -8.1977833194175052e+00], dtype=np.double)
    lat = 31.42807566666667
    lon = 120.63983669444444
    found, rvec, tvec = cv2.solvePnP(object_3d_points, object_2d_point, camera_matrix, dist_coefs)
    np.savez('zuzhou.npz', rvec=rvec, tvec=tvec,mtx=camera_matrix,dist=dist_coefs,lat = lat,lon=lon)
    #npzfile = np.load('dahua_position.npz')
    #rvec = npzfile['rvec']
    #tvec = npzfile['tvec']
    print(rvec)
    print(tvec)
    print(' ')
    return rvec,tvec

if __name__ == '__main__':
    points=[[615,220,1]]
    for i in range(len(points)):
        rvec,tvec=pixel_to_world(points[i])
